foundEnd = false

function goodPoint(x, y)
	-- if the point is out of bounds, it is not valid
	if (x>600 or y>500) then
		return false
	end
    for i,v in ipairs(terrain) do
        -- If the point is within a square, it is not valid.
        if ((v.x <= x  and x <= v.x+v.w) and (v.y <= y and y <= v.h + v.y)) then 
            return false;
        end
    end
    return true;
end 

-- check to see if all points on the line are good points
function goodLine(x1, y1, x2, y2)
	if x1 == x2 then
		if y1 == y2 then return goodPoint(x1, y1) end
		if y1 < y2 then
			for i=y1+1, y2 do
				if not goodPoint(x1, i) then
					return false
				end
			end
		else
			for i=y2+1, y1 do
				if not goodPoint(x1, i) then
					return false
				end
			end
		end
		return true
	end
	if x1 < x2 then
		for i=x1+1,x2 do
			local y = (y1 + ((y2-y1)/(x2-x1))*(i-x1))
			if not goodPoint(i, y) then
				return false
			end
		end
	else
		for i=x2+1,x1 do
			local y = y1 + ((y2-y1)/(x2-x1))*(i-x1)
			if not goodPoint(i, y) then
				return false
			end
		end
	end
	return true
end

function pathDistance(tab)
	local distance = 0
	for i,v in ipairs(tab) do
		if v.nextX ~= nil then
			distance = distance + getDistance(v.x, v.y, v.nextX, v.nextY)
		end
	end
	return distance
end

function cleanPath(distance, endX, endY, right)
	local continue = true
	local current = waypoints[1]
	while true do 
		table.insert(finalPoints, current)
		local previous = current
		local start = false
		for i,v in ipairs(waypoints) do
			local dist = getDistance(v.x, v.y, endX, endY) < distance
			if dist then
				continue = false
				table.insert(finalPoints, v)
				break
			end
			if v.x == current.x and v.y == current.y then
				start = true
			end 
			if start then
				local good     = goodLine(current.x, current.y, v.x, v.y)
				if not good then
					table.insert(finalPoints, previous)
					break
				end
				previous = v
			end
		end
		if not continue then
			break
		end
		current = previous
	end
	for i,v in ipairs(finalPoints) do
		if v.nextX ~= nil and finalPoints[i+1] ~= nil then
			v.nextX = finalPoints[i+1].x
			v.nextY = finalPoints[i+1].y
		end
	end 
	return 0
end

function findPath(x1, y1, x2, y2)
	local startAngle = calculateAngle(x1,y1,x2,y2)
	foundEnd = false
    waypoints = {}
	finalPoints = {}
	ShanesPath(x1, y1, 1, x2, y2, true)
	wayDist = pathDistance(waypoints)
	finDist = pathDistance(finalPoints)
	pathRunning = false
end

function ShanesPath(startX, startY, distance, endX, endY, right)
    if getDistance(startX, startY, endX, endY) < distance then
		-- give the endpoint a waypoint
		local current = createWayPoint(startX, startY)
		return cleanPath(distance, endX, endY, right)
    end
    local current = createWayPoint(startX, startY)
    local angle   = calculateAngle(startX, startY, endX, endY)
    local nextX   = distance*math.cos(angle) + startX
    local nextY   = distance*math.sin(angle) + startY
    while not goodPoint(nextX, nextY) do
		if right then
			angle = angle + 5
		else
			angle = angle - 5
		end
        nextX   = distance*math.cos(angle) + startX
        nextY   = distance*math.sin(angle) + startY
    end
    current.nextX = nextX
    current.nextY = nextY
    ShanesPath(nextX, nextY, distance, endX, endY, right)
end

function recursePath(x1,y1,angle,x2,y2,ttl)
	local self = createWayPoint(x1,y1)
	self.index = #waypoints
	angle = calculateAngle(x1,y1,x2,y2)
    self.children = 0
	
	--if we are there return 1
	if getDistance(x1,y1,x2,y2) < 40 then
		endPoint = self
		foundEnd = true
        self.children = 1
		return self
	end
	
	--if dead return -1
	if ttl <= 0 then
        self.children = -1
		return self
	end
	
	local i = 0
    local attempts = {}
	
    while math.abs(i) < 20 do
		if foundEnd == true then
			self.children = -1
			return self
		end
		local xx = 0
		local yy = 0
		xx,yy = move(x1,y1,angle + (i*math.pi/20),50)
		local spaceFree = true
		
        if xx < 0 or yy < 0 then
			spaceFree = false
		end
		
        for i,v in ipairs(terrain) do
			if squareCollision(xx,yy,v,8) == true or squareCollision((xx+x1)/2,(yy+y1)/2,v,8) == true then
				spaceFree = false
			end
		end
		
		for i,v in ipairs(waypoints) do
			if self ~= v and getDistance(xx,yy,v.x,v.y) < 20 then
				spaceFree = false
			end
		end
		
		if spaceFree == true then
			--local result = recursePath(xx,yy,angle + i*math.pi/360,x2,y2,ttl-1)
            local tryThis = recursePath(xx,yy,angle + i*math.pi/20,x2,y2,ttl-1)
            tryThis.parent = self
            if tryThis ~= nil and tryThis.children ~= -1 then
                table.insert(attempts, tryThis)
            end
               createWayPoint(xx,yy)
		end
 
		if i < 0 then
			i = (i-1) * -1
		else
			i = (i + 1) * -1
		end
	end
    
    local bestPath = {}
    bestPath.children = -1
        
    for i,v in ipairs(attempts) do
        if v.children ~= nil then
            if v.children <= bestPath.children and bestPath.children ~= -1 and v.children ~= -1 then
                bestPath = v
            end
        end
    end

    self.info = 'attempts:'..#attempts
    self.next = waypoints[bestPath.index]
    self.children = bestPath.children + 1
	return self
end